#!/usr/bin/python3
# -*- coding: utf-8 -*-
import numpy as np

# 靶标坐标系，默认和相机系重合
# 点从左至右,单位m    x周指向右，y周指向下，原点在中间的LED
# # 一米靶标
# plane_real_ptL = np.array([
#     [-0.50, 0.00, 0],
#     [-0.20,-0.10, 0],
#     [ 0.00, 0.00, 0],
#     [ 0.20,-0.10, 0],
#     [ 0.50, 0.00, 0]
# ])
# # 实际飞机 x周指向右，y周指向下the origin of the coordinate system is in the end of the body of the plane
# plane_real_ptL = np.array([
#     [-2.425, -0.35, 1.87],
#     [-0.75, -0.31, 0.18],
#     [0, 0.12, 0.25],
#     [0.750, -0.310, 0.180],
#     [2.425, -0.35, 1.87]   
# ])
# 5m
plane_real_ptL = np.array([
    [-2.5, 0, 0],
    [-0.35, -1, -0.35],
    [0, -1, 0],
    [0.35, -1, -0.35],
    [2.5, 0, 0]   
])
# # 10m
# plane_real_ptL = np.array([
#     [-5, 0, 0],
#     [-0.35, -1, -0.35],
#     [0, -1, 0],
#     [0.35, -1, -0.35],
#     [5, 0, 0]   
# ])
ar1 = np.linalg.norm(plane_real_ptL[0,:]-plane_real_ptL[1,:])
ar2 = np.linalg.norm(plane_real_ptL[1,:]-plane_real_ptL[2,:])
ar3 = np.linalg.norm(plane_real_ptL[2,:]-plane_real_ptL[3,:])
ar4 = np.linalg.norm(plane_real_ptL[3,:]-plane_real_ptL[4,:])
ar5 = np.linalg.norm(plane_real_ptL[0,:]-plane_real_ptL[2,:])
ar6 = np.linalg.norm(plane_real_ptL[2,:]-plane_real_ptL[4,:])
ar7 = np.linalg.norm(plane_real_ptL[1,:]-plane_real_ptL[2,:])
ar8 = np.linalg.norm(plane_real_ptL[2,:]-plane_real_ptL[3,:])
plane_real_val = np.array([ar1/ar2,ar2/ar3,ar3/ar4,ar5/ar6,ar7/ar8])

r_drogue = 0.22  #real drogue

t_bm = np.array([[0,0,2]]).T   #relative position between O_b and O_m
  

# #最终购买镜头  20/75/180 combination
# #长焦
# camK_lf = np.array([
#     [7581.7057, 0, 1130.6934],
#     [0, 7547.8008, 925.0308],
#     [0, 0, 1]
# ])
# distortion_coeffs_lf = np.array([
#     [0.0353, 2.3703, 0, 0, 0]
# ])
# #短焦
# camK_sf = np.array([
#     [1810.0735, 0 ,1275.4298],
#     [0, 1809.7697, 978.0254],
#     [0,0,1]
# ])
# distortion_coeffs_sf = np.array([
#     [-0.1695,0.0863,0 ,0,0]
# ])
# #鱼眼
# mapping_coeffs = np.array([[776.4652, -0.00042093, 5.5999e-08, -8.5335e-11]])
# stretch_matrix = np.eye(2,2)
# distortion_center = np.array([[1184.4048, 1072.8291]])  #都要写成矩阵

# 30/100/180 combination
camK_lf = np.array([
    [4803.9642, 0, 1108.7176],
    [0, 4810.1132, 1119.2559],
    [0, 0, 1],
])
distortion_coeffs_lf = np.array([
    [0.0718, -0.598, 0, 0, 0]
])
camK_sf = np.array([
    [1371.3490, 0, 1183.0822],
    [0, 1382.8809, 1146.4064],
    [0, 0, 1],
])
distortion_coeffs_sf = np.array([
    [-0.1053, 0.1106, 0, 0, 0]
])
mapping_coeffs = np.array([
    [752.3982, -0.000392488, 1.7830e-09, -6.4349e-11]
])
stretch_matrix = np.eye(2,2)
distortion_center = np.array([
    [1222.1976, 1056.1932]
])



# 世界系为ENU则有靶标系至世界系的转换矩阵
R_wt = np.array([
    [0,0,1],
    [-1,0,0],
    [0,-1,0],
])

# 相机系到FLU机体系的旋转矩阵
R_bc = np.array([
    [0,0,1],
    [-1,0,0],
    [0,-1,0],
])
R_cb = R_bc.T
